GPU Accelerated Parallel Occupancy Voxel Based ICP for Position Tracking

Tracking the position of a robot in an unknown environment is an important problem in robotics. Iterative closest point algorithms using range data are commonly used for position tracking, but can be computationally intensive. We describe a highly parallel occupancy grid iterative closest point position tracking algorithm designed for use on a GPU, that uses an Extended Kalman Filter to estimate motion between scans to increase the convergence rate. By exploiting the hardware structure of GPUs to rapidly find corresponding points and by using an occupancy map structure that can be safely modified in parallel with little synchronisation, our algorithm can run substantially faster than CPU based occupancy grid position tracking. We reduce the runtime from an average of 33ms to just 3ms on a commodity GPU, allowing our algorithm to use more dense and more frequent data, resulting in improved accuracy. Additionally, our solution can use a much larger occupancy grid without significantly impacting runtime.